#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

int main(int argc,char **argv)
{
    Eigen::Matrix3d rotation_matrix= Eigen::Matrix3d::Identity();
    Eigen::AngleAxisd rotation_vector (M_PI/4,Eigen::Vector3d(0,0,1));
    cout.precision(3);
    cout << "rotation matrix = \n" << rotation_vector.matrix() << endl;
    rotation_matrix = rotation_verctor.toRotationMatrix();
    Eigen::Vector3d v (1,0,0);
    v_rotated = rotation_matrix * v;
    cout << "(1,0,0) after rotation =" << v_rotated.transpose() << endl;
    v_rotated = rotation_matrix * v;
    cout << "(1,0,0) after rotation =" << v_rotated.transpose() << endl;
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles (2,1,0);
    cout<< "yaw pithch roll = " << euler_angels.transpose()<<endl;
    Eigen::Isometry3d T = Eigen::IsoMetry3d::Identity();
    T.rotate (rotation_vector);
    T.pretranslate (Eigen::Vector3d(1,3,4));
    cout<< "Transform matrix = \n" << T.matrix()<<endl;
}
